#ifndef _PEAKS_
#define _PEAKS_

//#include <boost/foreach.hpp>
#include <vector>
#include <iostream>
#include <utility>
#include <valarray>
#include <cmath>
#include <stdio.h>
#include <gsl/gsl_multifit.h>
#include <boost/algorithm/minmax_element.hpp>
#include "Spline.h"


using std::pair;
using std::vector;
using std::valarray;
using std::endl;
using std::cout;
using std::pair;
using std::min;

template <class T> 
int SIGN(T val)
	{
	return (val<0)?(-1):(1);
	};
template <class T>
T MyRound(T a, int ndig=3) {
	T npf=pow(10.0,(T)ndig);
	return T( int(a*npf)/npf);
	}

class is_negative
	{
	public:
		bool operator() (const std::pair<double, double> & value) 
			{ return value.second<0; }
	};


typedef std::pair<int,bool> PeakType;
typedef vector<PeakType>::iterator itPeakType;
///////Template to find Peaks.///
template <class T=double>
class CPeaks{
public:
	CPeaks(vector<T> *xin,vector<T> *yin ,
		bool conv=false, int npeaks=4):x(xin),m_conv(conv), 
		y(yin), m_npeaks(npeaks), IsHasPeaks(false){
			/////////////////////////
			Np=(*x).size();     
			GetPeaks();
			/////////////////////////
		}

	void show()
		{
		if(IsHasPeaks)
			{      
			cout<<"======\nFound: "<< final_peaks.size()<< " peaks."<<endl;
			unsigned int i=0;
			for(i=0; i<final_peaks.size();i++)
				{
				cout<<final_peaks[i].first<<" "<<final_peaks[i].second<<endl;	
				}
			}
		else  cout<<"======\nWarning -> No Peaks " <<endl;
		cout<<"\n======"<<endl;
		}

	~CPeaks(){}
	void  GetPeaks(void );
	///////////////////////////

	///////////////////////////
	void  BrutPeaks(void )
		{

		m_peak_index.clear();
		vector<T> yd;
		CSpline<T> spline(
			(T*)&(*x)[0],
			(T*)&(*y)[0],
			Np,"linear "); 

		for(int i =0; i<Np;i++)
			yd.push_back( spline.yp( (*x).at(i)));

		int sign=0, old_sign=SIGN<double>(MyRound((*y).at(0))-MyRound((*y).at(1)));

		for(int i =1; i<Np-1;i++)
			{

			sign=SIGN<double>( MyRound((*y).at(i))-MyRound((*y).at(i+1)) );
			if(sign!=old_sign)
				{	  
				m_peak_index.push_back(i);
				old_sign=sign;

				}
			}
		//Filter Peacks
		// Try to fit and get pos
		/////////////////////////

		double Pwidth=0.1;
		vector<double> ymin, ymax;
		for(unsigned int i =0; i<m_peak_index.size();i++)
			{
			gsl_matrix *cX, *ccov;
			gsl_vector *cy, *cw, *cc;
			double chisq ;
			int n =1000;
			cX = gsl_matrix_alloc (n, 3);
			cy = gsl_vector_alloc (n);
			cw = gsl_vector_alloc (n);
			cc = gsl_vector_alloc (3);
			ccov = gsl_matrix_alloc (3, 3);
			/////////////////////////
			CSpline<T> flipspline(
				(T*)&(*y)[0],
				(T*)&(*x)[0],
				Np,"linear ");  
			int ii=m_peak_index[i];
			//	 cout<<i<<")"<<ii<< " do peak correction: "<<(*x).at(ii)<<" "<<(*y).at(ii)<<endl;
			double xl=max((*x).at(ii)-Pwidth,0.0), xr=min(xl+2*Pwidth, 1.0);
			//	 FILE *FF=fopen("tt.txt", "w");

			for(int j=0;j<n;j++)
				{
				double xi, yi, ei;	     
				xi=(xr-xl)*j/double(n)+xl;
				yi=spline.y(xi);
				ei=1;
				gsl_matrix_set (cX, j, 0, 1.0);
				gsl_matrix_set (cX, j, 1, xi);
				gsl_matrix_set (cX, j, 2, xi*xi);

				gsl_vector_set (cy, j, yi);
				gsl_vector_set (cw, j, 1.0/(ei*ei));
				//	     fprintf(FF, "%g %g\n", xi, yi);
				}
			//	 fclose(FF);
				{
				gsl_multifit_linear_workspace * work 
					= gsl_multifit_linear_alloc (n, 3);
				gsl_multifit_wlinear (cX, cw, cy, cc, ccov,
					&chisq, work);
				gsl_multifit_linear_free (work);
				}
#define lC(i) (gsl_vector_get(cc,(i)))
#define lX(i) (gsl_matrix_get(cX,(i),(1)))
#define COV(i,j) (gsl_matrix_get(ccov,(i),(j)))    
				//	 printf ("#range[%g:%g] best fit: Y = %g + %g * x + %g * x*x\n", 
				//        xl, xr,lC(0), lC(1), lC(2));
				//printf ("# chisq = %g\n", chisq);
				std::vector<double> Yrange;
				for(int j=0;j<n;j++)
					{
					double xi=lX(j);
					Yrange.push_back(lC(0)+lC(1)*xi+lC(2)*xi*xi);
					}

				typedef  std::vector<double>::iterator T_indexID;
				pair< T_indexID, T_indexID > mma =
					boost::minmax_element(Yrange.begin(), Yrange.end() );

				if(*(mma.first)!=Yrange.front() && 
					*(mma.first)!=Yrange.back() 	    
					) 
					{
					ymin.push_back(*(mma.first));	     
					m_marked_peak_index.push_back(std::make_pair(ii, false));
					}

				if(*(mma.second)!=Yrange.front() && 
					*(mma.second)!=Yrange.back()  	    
					){
						m_marked_peak_index.push_back(std::make_pair(ii, true));
						ymax.push_back(*(mma.first));
					}

				gsl_matrix_free (cX);
				gsl_vector_free (cy);
				gsl_vector_free (cw);
				gsl_vector_free (cc);
				gsl_matrix_free (ccov);
#undef lC
#undef COV
			}

			{

			int Imi=0, Ima=0;
			double pdif=*(boost::minmax_element((*y).begin(),(*y).end() ).first);
			pdif=(*(boost::minmax_element((*y).begin(),(*y).end() ).second) - pdif)*0.5;


			for(itPeakType j=m_marked_peak_index.begin();j!=m_marked_peak_index.end();j++)
				{
				double xp=(*x).at((*j).first);
				double yp=(*y).at((*j).first);
				final_peaks.push_back(std::make_pair(xp,yp));	
				}

			double xin=final_peaks[0].first;
			for(unsigned int j=1;j<final_peaks.size();j++)
				{
				if(final_peaks[j].second>0)
					if((final_peaks[j].first-xin) < 0.1)
						{
						final_peaks[j-1].first=(final_peaks[j].first+xin)*0.5;
						xin=final_peaks[j-1].first;
						if(final_peaks[j-1].second<0) final_peaks[j-1].second*= -1;	     
						final_peaks[j].second *= -1;	     
						j=1;
						}else
							xin=final_peaks[j].first;
				}

			}
			vector<pair<double, double> >::iterator new_end=remove_if ( final_peaks.begin(),  final_peaks.end(),is_negative()); 
			final_peaks.erase(new_end, final_peaks.end()); 



		}
	vector<int> m_peak_index; 
	vector< PeakType > m_marked_peak_index; 
	vector<pair<double, double> > final_peaks;
private:
	int Np;
	int m_npeaks;
	T m_fac;

	vector<T> *x;
	vector<T> *y;
	bool IsHasPeaks;
	bool m_conv;

	bool CrossingsTest(int val)
		{
		return (val==2 || val == -2);
		}
	};
template<class T>
void  CPeaks<T>::GetPeaks(void )
	{

	/////////////////////////
	// Main Algorithm!!! 
	int   n_crossings = 0 , xpeaks = 0 , fwhm = 0;
	int NCOEF=256;
	CSpline<T> spline(
		(T*)&(*x)[0],
		(T*)&(*y)[0],
		Np,"cubic ");

	T yd;
	valarray<int> value_sign(Np);
	valarray<int> diff_sign(Np);
	vector<int>  wh_cross,indices;
	for(int i =0; i<Np;i++)
		{
		if(m_conv)
			yd=(*y).at(i);
		else
			yd=spline.yp( (*x).at(i));
		value_sign[i]=int(2*(yd > 0.0) - 1);//2*(yd gt 0d) - 1			     
		}

	for(int i =0; i<Np-1;i++)
		{
		diff_sign[i] = value_sign[i+1]-value_sign[i];
		if(CrossingsTest(diff_sign[i]))
			wh_cross.push_back(i);
		}

	n_crossings=wh_cross.size();

	if(n_crossings>0)
		{
		indices.resize(n_crossings);
		for(int i =0; i<n_crossings-1;i++)
			indices[i]= (int)(0.5*(wh_cross[i]*2-1));


		T ymin=1e10;
		vector<T> ymax;
		for(unsigned int i=0;i<indices.size();i++)
			{
			T value = spline.y(indices[i]);
			ymax.push_back( value);
			ymin=min( value, ymin);	     
			}


		typedef std::vector<double>::iterator typeVecIt;
		typeVecIt this_max;
		vector<int> best_index;

		for(int i = 0;i<m_npeaks-1;i++)
			{
			this_max = std::max_element(ymax.begin(),ymax.end());

			best_index.push_back(indices[std::distance(ymax.begin(), this_max)]);
			*this_max=ymin;
			//cout<<" AAAhaa>>>>>>>>>>> "<<m_npeaks<< " " <<best_index[i]<<
			//  " "<<(*x).at(best_index[i])<<endl;
			}
		m_peak_index.resize(best_index.size());
		std::copy( best_index.begin(),  best_index.end(),  m_peak_index.begin());
		m_npeaks=m_peak_index.size();
		IsHasPeaks=true;
		}else
			IsHasPeaks=false;
		///////////////////////// 


	};

#endif
